#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <stdlib.h>

#include "spark_uart.h"
#include "spark_arm.h"


ArmStatus g_arm_status=0;
char recv_status[80];

//机械臂发送指令
void Arm_Cmd(char *data)
{
    Uart_SentData(data,sizeof(data));
}
//机械臂抓取
void Arm_Grasp(void)
{
    Arm_Cmd("5");
}
//机械臂放置 (第一层)
void Arm_Release(void)
{
    Arm_Cmd("6");
}
//机械臂放置 (第二层)
void Arm_Release_two(void)
{
    Arm_Cmd("7");
}

//机械臂复位
// void Arm_Reset(void)
// {
//     Arm_Cmd("6");
// }

//单个键控制机械臂
// void Arm_Control(void)
// {
//     if(g_arm_status == ARM_STATUS_ERROR)
//     {
//         Arm_Reset();
//     }
//     else if(g_arm_status == ARM_STATUS_RELEASE)
//     {
//         Arm_Grasp();
//     }
//     else if(g_arm_status == ARM_STATUS_GRASP)
//     {
//         Arm_Release();
//     }

// }

//获取机械臂状态
void Get_Arm_Status(char* status)
{
    if(g_arm_status == ARM_STATUS_GRASP)
    {
        strcpy(status,"1");
        printf("the arm status is GRASP\r\n");
    }
    else if(g_arm_status == ARM_STATUS_RELEASE)
    {
        strcpy(status,"0");
        printf("the arm status is RELEASE\r\n");
    }
    else
    {
        strcpy(status,"-1");
        printf("the arm status is UNKNOW\r\n");
    }
}
    
//更新机械臂状态
// void Update_Arm_Status(void)
// {
//     Uart_ReceiveData(recv_status,sizeof(recv_status));
//     printf("recv_status:%s\r\n",recv_status);
//     if(!strcmp(recv_status,"0"))
//     {
//         g_arm_status = ARM_STATUS_RELEASE;
//     }
//     else if(!strcmp(recv_status,"1"))
//     {
//         g_arm_status = ARM_STATUS_GRASP;
//     }
//     else
//     {
//         g_arm_status =ARM_STATUS_ERROR;
//     }
//     bzero(recv_status,sizeof(recv_status));
// }

